#ifndef _INCL_CFLYINGROBOT

#define _INCL_CFLYINGROBOT

#include <mrpt/poses/CPose3D.h>
#include <mrpt/utils/CTicTac.h>
#include "CFilt2.h"

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::utils;

class CFlyingRobot
{
    public :
    
      CFlyingRobot();
      ~CFlyingRobot();
      
      void SetCommands(double phi,double theta, double psi,float height);
      
      CPose3D getRenderPose();
      
      void simulate();
      
      void init();
      
      CPose3D m_pose;
      double getVx();
      double getVy();
      double getVz();  
       
       double acc[3];
      
      bool is_realtime;
      
      bool first_exec;
      
    private :
    
    double x[3];
        double v[3];
        
        
        
        double Ts ;
        
        double phi_c_rad;
        double theta_c_rad;
        double psi_c_rad;
        
        double h_c_m;
        
        
        CTicTac		tictac;
        
        CFilt2 f_phi;
        CFilt2 f_theta;
        CFilt2 f_psi;
        
        CFilt2 f_h;
        
        
};


#endif
